///////////////////////////////////////////////////////////////////////////////
//
// Lynxmotion SSC-32 Servo Controller Service
//
// Based on code from the Lynxmotion web site arm.1.2.zip
// Modified with the permission of Lynxmotion
//
// Updated Nov-2007 by Trevor Taylor, Software Technology
//
// This code is Open Source
//
//
// Change Log:
//
// Initialization changed so that the elbow of the Lynx 6 arm
// will be horizontal, i.e. at right-angles to the upper arm.
// This is *NOT* a good way to do it. The initial positions
// should be in a config file. This change makes this code
// specific to the Lynx 6 arm!!!
//
// Fixed a bug in StreamQuery (in SSC32Comm.cs) that was giving
// bad data for queries on the joint angles.
//
// NOTE: The arm might jump when the SSC-32 is turned on. If you
// power the arm and the SSC-32 separately then you can avoid
// this by turning on the SSC-32 first. The arm will also jump
// during the initialization here because the servos are all set
// to initial angles. This is unavoidable because the SSC-32 has
// no way to tell what the servo positions are at the outset, so
// it just has to set them to the midway point.
//
// Contract Identifier and Namespace modified in Feb 2008 to avoid a conflict
// with the CoroWare ClassPack which also includes an SSC-32 service
//
///////////////////////////////////////////////////////////////////////////////

using System;
using System.Collections.Generic;
using System.Collections.Specialized;
using System.Security.Permissions;
using System.Text;
using System.Net;

using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.Core.DsspHttp;
using Microsoft.Dss.Core.DsspHttpUtilities;
using Microsoft.Dss.ServiceModel.Dssp;
using dssp = Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using W3C.Soap;

using permissions = System.Security.Permissions;
using System.ComponentModel;

namespace ProMRDS.Robotics.Lynxmotion.Ssc32
{
 
    [DisplayName("LynxMotion SSC32 Controller Board")]
    [Description("Supports communication with the LynxMotion SSC32 board over a serial port.")]
    [Contract(Contract.Identifier)]
    [permissions.PermissionSet(permissions.SecurityAction.Demand, Unrestricted = true)]
    internal class SSC32Service : DsspServiceBase
    {
        private SSC32State _state = new SSC32State();

        // A separate class for handling low-level communication with the SSC32 board
        private SSC32Comm _comm = new SSC32Comm();

        [EmbeddedResource("ProMRDS.Robotics.Lynxmotion.Ssc32.ssc32.xslt")]
        string _transform = null;
        DsspHttpUtilitiesPort _httpUtilities = new DsspHttpUtilitiesPort();

        [ServicePort("/SSC32", AllowMultipleInstances=false)]
        private SSC32Operations _mainPort = new SSC32Operations();

        // TT Nov-2007 - Add an initial time to the startup command
        // so that the arm does not jump
        private int _initialTime = 500;

        // TT Nov-2007 - Got tired of waiting for 32 channels ...
        // Must be less than or equal to NUM_CHANNELS.
        // Should be in a config file. Maybe in the next version.
        private int _maxChannels = 6;

        // Flag that is only set once the service is ready
        private bool _initialized = false;


        public SSC32Service(DsspServiceCreationPort creationPort) : 
                base(creationPort)
        {
        }

        protected override void Start()
        {
            _httpUtilities = DsspHttpUtilitiesService.Create(this.Environment);

            ActivateDsspOperationHandlers().CombineWith(
                Arbiter.Interleave(
                    new TeardownReceiverGroup(),
                    new ExclusiveReceiverGroup
                    (
                        Arbiter.Receive<SendSSC32Command>(true, _mainPort, SSC32CommandHandler)
                    ),
                    new ConcurrentReceiverGroup
                    (
                    ))
            );

            // Display browser accesible Service URI
            LogInfo("Service uri: " + ServiceInfo.Service);

            SpawnIterator(InitState);
        }

        // Initialize all the servo pulseWidths to 1500
        private IEnumerator<ITask> InitState()
        {
            int numChan = (_maxChannels <= SSC32State.NUM_CHANNELS) ? _maxChannels : SSC32State.NUM_CHANNELS;
            int[] channels = new int[numChan];
            int[] pulseWidths = new int[numChan];

            LogInfo(LogGroups.Console, "Initializing Joints");

            for (int i = 0; i < numChan; i++)
            {
                channels[i] = i;
                pulseWidths[i] = 1500;
                // TT Nov-2007 - Special case for Lynx L6 Arm
                // IMPORTANT: The Lynx L6 arm has a 20 degree offset on the
                // attachment of the upper arm to the lower arm. To start
                // of "square" it is necessary to adjust the initial pulse
                // value. This also helps by reducing the amount that the
                // arm jumps by. However, this value is specific to the
                // Lynx L6. If you want to use this service with another
                // arm, then you either need to modify the code below or
                // add a config file (which is the proper solution).
                if (i == 2)
                    pulseWidths[i] = 1722;

                // Debugging
                LogInfo(LogGroups.Console, "Joint " + i);

                // Initialize the state too
                _state.SetServoPulseValue(i, pulseWidths[i]);

                // TT Nov-2007 - Slow down the initial moves by moving only one servo at a time.
                // This does not prevent the arm from jerking, but it reduces the problem.
                // IMPORTANT NOTE:
                // From the SSC-32 V2.0 Users Manual
                // Don't issue a speed or time command to the servo controller as the first
                // instruction. It will assume it needs to start at 500uS and will zip there
                // as quickly as possible. The first positioning command should be a normal
                // "# <ch> P <pw>" command. Because the controller doesn't know where the
                // servo is positioned on power up it has to be this way.
                SSC32ServoMove moveRequest = new SSC32ServoMove(channels[i], pulseWidths[i], 500, 1000);
                SendSSC32Command moveCommand = new SendSSC32Command(moveRequest);
                _mainPort.Post(moveCommand);

                yield return Arbiter.Choice(
                    moveCommand.ResponsePort,
                    delegate(SSC32ResponseType response)
                    {
                    },
                    delegate(Fault f)
                    {
                        LogError(f);
                        LogError("Servos cannot be initialized.");
                    }
                );
                // Wait for settling time
                yield return Arbiter.Receive(
                    false,
                    TimeoutPort(_initialTime),
                    delegate(DateTime timeout) { });
            }

            // Insert ourselves into the directory so that others can find us
            // now that the arm should be in its initial position
            DirectoryInsert();
            _initialized = true;

            // TT - Debugging
            LogInfo(LogGroups.Console, "SSC32 Service Initialized\r\n");
        }

        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> GetHandler(Get get)
        {
            int counter = 0;
            while (!_initialized)
            {
                // Wait for the service to start up properly so that we
                // do not return bad data, and also this will help other
                // services to synchronize if they send a Get request
                yield return Arbiter.Receive(
                    false,
                    TimeoutPort(_initialTime),
                    delegate(DateTime timeout) { });

                // TT - Debugging
                LogError(LogGroups.Console, "SSC32 Get before Init complete\r\n");

                // Don't wait forever. Eventually we have to give up!
                counter++;
                if (counter > 20)
                {
                    get.ResponsePort.Post(new Fault());
                    yield break;
                }
            }

            yield return Arbiter.Choice(
                UpdateStateFromPulseQuery(),
                delegate(SuccessResult success)
                {
                    get.ResponsePort.Post(_state);
                },
                delegate(Exception ex)
                {
                    get.ResponsePort.Post(Fault.FromException(ex));
                }
            );
        }

        #region HTTP Get and Post

        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> HttpGetHandler(HttpGet httpget)
        {
            HttpListenerRequest request = httpget.Body.Context.Request;
            HttpListenerResponse response = httpget.Body.Context.Response;

            yield return Arbiter.Choice(
                UpdateStateFromPulseQuery(),
                delegate(SuccessResult success)
                {
                    //LogInfo("State update");
                    HttpResponseType rsp = new HttpResponseType(HttpStatusCode.OK, _state, _transform);
                    httpget.ResponsePort.Post(rsp);       
                },
                delegate(Exception ex)
                {
                    LogError(ex);
                    httpget.ResponsePort.Post(Fault.FromException(ex));
                }
            );
        }

        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> HttpPostHandler(HttpPost httpPost)
        {
            // TT Jul-2008 - Change to parameter for July CTP of 2.0
            ReadFormData request = new ReadFormData(httpPost);
            _httpUtilities.Post(request);
            NameValueCollection collection = null;

            yield return Arbiter.Choice(
                request.ResultPort,
                delegate(NameValueCollection items)
                {
                    collection = items;
                },
                delegate(Exception e)
                {
                    LogError(e);
                }
            );

            if (collection == null)
                yield break;

            string operation = collection["operation"];
            if (operation == "pos")
            {
                int[] channels = new int[32];
                int[] pulseWidths = new int[32];
                int[] speeds = new int[32];
                try
                {
                    for (int i = 0; i < 32; i++)
                    {
                        channels[i] = i;
                        pulseWidths[i] = int.Parse(collection["ServoPulse" + i]);
                        speeds[i] = 500;
                    }
                }
                catch (Exception ex)
                {
                    LogError(ex);
                    yield break;
                }

                SSC32ServoMove moveRequest = new SSC32ServoMove(channels,pulseWidths,speeds,-1);
                SendSSC32Command moveCommand = new SendSSC32Command(moveRequest);
                SSC32CommandHandler(moveCommand);
                yield return Arbiter.Choice(moveCommand.ResponsePort,
                    delegate(SSC32ResponseType response)
                    {
                    },
                    delegate(Fault f)
                    {
                        LogError(f);
                    });
            }
            
            HttpResponseType rsp = new HttpResponseType(HttpStatusCode.OK, _state, _transform);
            httpPost.ResponsePort.Post(rsp);
        }
        #endregion

        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual void SSC32CommandHandler(SendSSC32Command submit)
        {
            SSC32Command command = submit.Body;
            string commandString;
            switch (command.CommandType)
            {
                case SSC32Commands.ServoMove:
                    //use helper to create command string 
                    commandString = SSC32Helper.GenerateSSC32ServoMove((SSC32ServoMove)command);
                    if (commandString == null)
                    {
                        LogError("Servo Move command invalid.");
                        submit.ResponsePort.Post(Fault.FromCodeSubcodeReason(
                            FaultCodes.Receiver,
                            dssp.DsspFaultCodes.UnknownEntry,
                            "Servo Move command invalid."));
                        break;
                    }
                    // TT - Debugging
                    //LogInfo(LogGroups.Console, "SSC32 Command: " + commandString + "\r\n");

                    int[] channels = ((SSC32ServoMove)command).Channels;
                    int[] pulseWidths = ((SSC32ServoMove)command).PulseWidths;

                    //sends the command string to the controller
                    bool success = _comm.StreamCommand(commandString);
                    if (success)
                    {
                        //update ssc32 state
                        for (int i = 0; i < channels.Length; i++)
                        {
                            _state.SetServoPulseValue(channels[i], pulseWidths[i]);
                        }
                        submit.ResponsePort.Post(new SSC32ResponseType());
                    }
                    else
                    {
                        LogError("Servo Move command failed.");
                        submit.ResponsePort.Post(
                            Fault.FromCodeSubcodeReason(
                                FaultCodes.Receiver,
                                dssp.DsspFaultCodes.UnknownEntry,
                                "Servo Move command failed."));
                    }                                        
                    break;

                case SSC32Commands.QueryPulseWidth:
                    //use helper to create command string 
                    commandString = SSC32Helper.GenerateSSC32QueryPulseWidth((SSC32QueryPulseWidth)command);
                    if (commandString == null)
                    {
                        LogError("Query Pulse Width command invalid.");
                        submit.ResponsePort.Post(
                            Fault.FromCodeSubcodeReason(
                                FaultCodes.Receiver,
                                dssp.DsspFaultCodes.UnknownEntry,
                                "Query Pulse Width command invalid."));
                        break;
                    }
                    
                    int[] Querychannels = ((SSC32QueryPulseWidth)command).Channels;
                    int size = Querychannels.Length;
                    //sends the query command to the controller
                    byte[] response = _comm.StreamQuery(commandString, size);

                    if (response == null)
                    {
                        LogError("Unable to query all the channels.");
                        submit.ResponsePort.Post(
                            Fault.FromCodeSubcodeReason(
                                FaultCodes.Receiver,
                                dssp.DsspFaultCodes.OperationFailed,
                                "Unable to query all the channels."));
                        break;
                    }
                    
                    int[] QueryPulseWidths = new int[size];
                    for (int i = 0; i < size; i++)
                    {
                        QueryPulseWidths[i] = (int)(response[i]) * 10;
                        //update the ssc32 state
                        _state.SetServoPulseValue(Querychannels[i], QueryPulseWidths[i]);
                    }
                    submit.ResponsePort.Post(new SSC32PulseWidthResponse(QueryPulseWidths));
                    break;

                default:
                    LogError("SSC32 command not recognized.");
                    submit.ResponsePort.Post(
                        Fault.FromCodeSubcodeReason(
                                FaultCodes.Receiver,
                                dssp.DsspFaultCodes.OperationFailed,
                                "SSC32 command not recognized."));
                    break;
            }
        }

        private SuccessFailurePort UpdateStateFromPulseQuery()
        {
            // Note: Timeout is defined in the serial port state object (defaults to 1 sec currently)
            SuccessFailurePort resultPort = new SuccessFailurePort();

            int[] channels = new int[SSC32State.NUM_CHANNELS];
            for (int i = 0; i < SSC32State.NUM_CHANNELS; i++)
            {
                channels[i] = i;
            }
            SSC32QueryPulseWidth queryRequest = new SSC32QueryPulseWidth(channels);
            SendSSC32Command queryCommand = new SendSSC32Command(queryRequest);
            SSC32CommandHandler(queryCommand);
            Activate(Arbiter.Choice(queryCommand.ResponsePort,
                delegate(SSC32ResponseType response)
                {
                    resultPort.Post(new SuccessResult());
                },
                 delegate(Fault fault)
                 {
                     resultPort.Post(new Exception(fault.Reason[0].ToString()));
                 }
            ));

            return resultPort;
        }
    }
}
